#include "../H/RobotAimer.h"
#define AREA_MINIMUM 500
//------------------------------------------------------------------------------
// METHOD:  RobotAimer::RobotAimer()
// Type:	Constructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
RobotAimer::RobotAimer(TankDrive *pTankDrive)
{
	printf("BEGIN:  RobotAimer(channel)\n");

	pDiskAimer     = new Victor(DIGITAL_SIDECAR_MODULE,DISK_AIMER_CH);
	pAngleSensor   = new DigitalInput(DIGITAL_SIDECAR_MODULE,ANGLE_SENSOR_CH);
	pTurningWheels = pTankDrive;

	SmartDashboard::init();

	/*
	pCamera->WriteBrightness(30);
	pCamera->WriteWhiteBalance(AxisCamera::kWhiteBalance_Hold);
	pCamera->WriteCompression(0);
	pCamera->WriteColorLevel(100);
	pCamera->WriteMaxFPS(10);
	pCamera->WriteResolution(AxisCamera::kResolution_320x240);
	*/

	sensitivityX = 10;
	sensitivityY = 10;
	upperBoundX  = 131 + sensitivityX;
	lowerBoundX  = 131 - sensitivityX;
	upperBoundY  = 160 + sensitivityY;
	lowerBoundY  = 160 - sensitivityY;
	shooterAngle = 20;
	robotAimedX  = false;
	robotAimedY	 = true;

	// Turn motor speed
	turnSpeed  = 0.21;
	raiseSpeed = 0.21;
}

//------------------------------------------------------------------------------
// METHOD:  RobotAimer::~RobotAimer()
// Type:	Destructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
RobotAimer::~RobotAimer()
{
	// Does nothing
}
//------------------------------------------------------------------------------
// METHOD:  RobotAimer::TurnOn()
// Type:	Method
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
bool  RobotAimer::ProcessImage()
{
	AxisCamera &camera = AxisCamera::GetInstance();
	// Configure camera settings
	camera.WriteResolution(AxisCameraParams::kResolution_320x240);
	camera.WriteBrightness(30);
	camera.WriteCompression(0);
	camera.WriteColorLevel(100);
	camera.WriteMaxFPS(10);
	camera.WriteWhiteBalance(AxisCamera::kWhiteBalance_Hold);

	if (camera.IsFreshImage())
	{
		ParticleFilterCriteria2 criteria[] = {
				{IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}
		};

		//Threshold threshold(0, 255, 0, 255, 60, 255);
		Threshold threshold(69, 255, 146, 255, 14, 253);

		ColorImage *image;
		camera.GetImage(image);
		BinaryImage *thresholdImage = image->ThresholdHSV(threshold);	// get just the green target pixels
		BinaryImage *convexHullImage = thresholdImage->ConvexHull(false);  // fill in partial and full rectangles
		BinaryImage *filteredImage = convexHullImage->ParticleFilter(criteria, 1);	//Remove small particles
		vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports();  // get the results

		int height = image->GetHeight();
		for ( unsigned i = 0; i < reports->size(); i++ )
		{
			ParticleAnalysisReport *r = &(reports->at(i));

			// Look for highest particle
			if ( r->center_mass_y < height )
			{
				height = r->center_mass_y;
				highestReport = r;
			}
		}

		// Set found or not
		if ( (int)reports->size()>0 )
		{
			// Target was found
			targetCaptured = true;
		}
		else
		{
			targetCaptured = false;
		}

		// be sure to delete images after using them
		delete reports;
		delete filteredImage;
		delete convexHullImage;
		delete thresholdImage;
		delete image;
	}

	SmartDashboard::PutBoolean("Target Captured: ", targetCaptured);
	return targetCaptured;
}
//------------------------------------------------------------------------------
// METHOD:  RobotAimer::TurnOff()
// Type:	Constructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
bool  RobotAimer::AimRobotX()
{
	printf("BEGIN:  RobotAimer::AimeRobotX()\n-----\n");

	actualX = highestReport->center_mass_x;

	if ( actualX <= upperBoundX && actualX >= lowerBoundX )  // Within bounds
		{
			pTurningWheels->SetDriveMotors(GRANNY_GEAR,STOP_MOTOR,STOP_MOTOR);
			robotAimedX = true;
		}
		else
		{
			/* TODO: AIMING ALGORITHM
			 * - Fine-tune the process of setting the motor speeds to reduce overshoot
			 *   That should minimize x-axis error
			 * - Maybe lock motor movement to this function (could be bad) or measure
			 *   how much the motor moves per pulse/tick/whatever
			 */
			if ( actualX > upperBoundX )					// Move left
			{
				pTurningWheels->SetDriveMotors(GRANNY_GEAR,REVERSE_SPEED,TURN_SPEED);
				robotAimedX = false;
			}

			if ( actualX < lowerBoundX )			// Move right
			{
				pTurningWheels->SetDriveMotors(GRANNY_GEAR,TURN_SPEED,REVERSE_SPEED);
				robotAimedX = false;
			}
		}

	SmartDashboard::PutBoolean("Robot Aimed (X)", robotAimedX);
	return robotAimedX;
}
//------------------------------------------------------------------------------
// METHOD:  RobotAimer::GetStatus()
// Type:	Constructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
bool  RobotAimer::AimRobotY()
{
	printf("BEGIN:  RobotAimer::AimRobotY()\n-----\n");
	int actualY = highestReport->center_mass_y;

	if ( actualY <= upperBoundY && actualY >= lowerBoundY )  // Within bounds
	{
		// Stop motors
		AutoAdjustShooterAngle(STOP_MOTOR);
		robotAimedY = true;
	}
	else
	{
		if( actualY > upperBoundY)
		{
			AutoAdjustShooterAngle(-raiseSpeed);
			robotAimedY = false;
		}
		if( actualY < lowerBoundY)
		{
			AutoAdjustShooterAngle(raiseSpeed);
			robotAimedY = false;
		}
	}
	SmartDashboard::PutBoolean("Robot Aimed(Y)", robotAimedY);
	return robotAimedY;
}
//------------------------------------------------------------------------------
// METHOD:  RobotAimer::AdjustShooterAngle()
// Type:	Constructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void  RobotAimer::AdjustShooterAngle(float target)
{
	printf("BEGIN:  RobotAimer::AimRobotY()\n-----\n");
	angleSensorReading = pAngleSensor->Get();

	target = int(((target*RATIO)+ CONST));

	if (shooterAngle > target)
	{
		pDiskAimer->Set(ANGLE_REVERSE_SPEED);
		if (!angleSensorReading && lastAngleSensorReading)
			shooterAngle--;
	}
	else if (shooterAngle < target)
	{
		pDiskAimer->Set(ANGLE_SPEED);
		if (!angleSensorReading && lastAngleSensorReading)
			shooterAngle++;
	}
	else
		pDiskAimer->Set(STOP_MOTOR);

	SmartDashboard::PutNumber("Angle Sensor Target", target);
	SmartDashboard::PutBoolean("Angle Sensor Reading", angleSensorReading);
	SmartDashboard::PutNumber("Shooter Angle: ", shooterAngle);
	lastAngleSensorReading = angleSensorReading;

	return;
}

//------------------------------------------------------------------------------
// METHOD:  RobotAimer::AdjustShooterAngle()
// Type:	Constructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void  RobotAimer::AutoAdjustShooterAngle(float speed)
{
	printf("BEGIN:  RobotAimer::AimRobotY()\n-----\n");
	angleSensorReading = (pAngleSensor->Get() * RATIO)+CONST;

	if (speed > 0)
	{
		pDiskAimer->Set(speed);
		if (!angleSensorReading && lastAngleSensorReading)
			shooterAngle++;
	}
	else
	{
		pDiskAimer->Set(speed);
		if (!angleSensorReading && lastAngleSensorReading)
			shooterAngle--;
	}

	SmartDashboard::PutBoolean("Angle Sensor Reading", angleSensorReading);
	SmartDashboard::PutNumber("Shooter Angle: ", shooterAngle);
	lastAngleSensorReading = angleSensorReading;
	return;
}

//------------------------------------------------------------------------------
// METHOD:  RobotAimer::AdjustShooterAngle()
// Type:	Constructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void  RobotAimer::SetInputToOutputFormula(float AIMER_CONST, float AIMER_RATIO)
{
	RATIO = AIMER_RATIO;
	CONST = AIMER_CONST;
	return;
}
